#ifndef __PCPT_COMM_ROS_H__
#define __PCPT_COMM_ROS_H__

#include <ros/ros.h>

#include "comm_msg/coor2DArray.h"
#include "comm_msg/movTargetsArray.h"
#include "comm_msg/visionDepthCamera.h"
#include "comm_msg/visionState.h"
#include "comm_msg/radarTargetArray.h"

#include "comm/comm_msg.h"
#include "comm/comm_base.h"
#include "comm/comm_define.h"
#include "comm/comm_file.h"
#include "comm/comm_car.h"
#include "comm/comm_base.h"
#include "comm/comm_map.h"
#include "comm/comm_dbg_sw.h"
#include "comm/comm_cfg_param.h"
#include "comm/comm_ros_base.h"
#include "comm/comm_ros_trc.h"
#include "comm/comm_msg_nav_state.h"
#include "comm/comm_topic_name.h"
#include "comm_extend/comm_base_extend.h"

#include "std_msgs/Header.h"
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <visualization_msgs/Marker.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include "pcpt_comm/pcpt_comm_base.h"
#include "pcpt_comm/pcpt_comm_car.h"

// 屏蔽库文件编译告警
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wreturn-type"

#include <sensor_msgs/point_cloud_conversion.h>

#pragma GCC diagnostic pop

using namespace std;

/**************************************************************************************
功能描述: 点云消息格式相互转化
修改记录:
**************************************************************************************/
class POINT_CLOUD_MSG_TRANSFORM_C
{
public:
    POINT_CLOUD_MSG_TRANSFORM_C(void){}
    ~POINT_CLOUD_MSG_TRANSFORM_C(void){}
    
    void MsgRosToMsgPcl(const sensor_msgs::PointCloud2 &inPut , pcl::PCLPointCloud2 &outPut);
    void MsgRosToMsgPcl(const sensor_msgs::PointCloud2 &inPut , pcl::PointCloud<MyPoint> &outPut);
    void MsgPclToMgRos(const pcl::PCLPointCloud2 &inPut, sensor_msgs::PointCloud2 &outPut);
    void MsgPclToMsgPcl(const pcl::PCLPointCloud2 &inPut, pcl::PointCloud<MyPoint> &outPut);
    void MsgPclToMsgRos(const pcl::PointCloud<MyPoint> &inPut, sensor_msgs::PointCloud2 &outPut);
    void MsgPclToMsgPcl(const pcl::PointCloud<MyPoint> &inPut, pcl::PCLPointCloud2 &outPut);
    void ConvertPointCloudToPointCloud2(const sensor_msgs::PointCloud &inPut, sensor_msgs::PointCloud2 &outPut);
    void ConvertPointCloud2ToPointCloud(const sensor_msgs::PointCloud2 &inPut, sensor_msgs::PointCloud &outPut);

    template<typename T> void AddHeaderToPCL(T &pCloud) const
    {
        std_msgs::Header header;
        header.stamp = ros::Time::now();
        header.frame_id = "base_link";
        pcl_conversions::toPCL(header, pCloud->header);
    }

    template<typename T> void AddHeaderFromPCL(T &pCloud) const
    {
        std_msgs::Header header;
        pcl_conversions::fromPCL(pCloud->header, header);
    }

private:

};

/**************************************************************************************
功能描述: 点云数据发布器
修改记录:
**************************************************************************************/
class POINT_CLOUD_MSG_SENDER_C
{
public:
    POINT_CLOUD_MSG_SENDER_C(ros::NodeHandle &nh, string pcTopicName)
      : hasSubscriber_(false)
    {
        publisher_ = nh.advertise<sensor_msgs::PointCloud2>(pcTopicName, 10);
    }

    /**************************************************************************************
    功能描述: 点云发布
    修改记录:
    **************************************************************************************/
    template<typename T> void Send(T &pCloud, const char* coorType) const
    {
        if (publisher_.getNumSubscribers() && pCloud->size()) {
            sensor_msgs::PointCloud2 pointCloudMsg;
            pcl::toROSMsg(*pCloud, pointCloudMsg);
            pointCloudMsg.header.stamp = ros::Time::now();
            pointCloudMsg.header.frame_id = coorType;
            publisher_.publish(pointCloudMsg);
        }
    }

    template<typename T> void Send(T &pCloud, string coorType) const
    {
        Send(pCloud, coorType.c_str());
    }
    
    template<typename T> void Send(T &pCloud) const
    {
        Send(pCloud, "base_link");
    }
    
    /**************************************************************************************
    功能描述: 点云发布(dimension 为2 压缩二维后发布)
    修改记录:
    **************************************************************************************/
    template<typename T> void Send(T &pCloud, int dimension) const
    {
        if (publisher_.getNumSubscribers() && pCloud->size() && dimension == 2) {
            for (UINT32 i = 0; i < pCloud->size(); ++i)
                pCloud->points[i].z = 0;
            
            sensor_msgs::PointCloud2 pointCloudMsg;
            pcl::toROSMsg(*pCloud, pointCloudMsg);
            pointCloudMsg.header.stamp = ros::Time::now();
            pointCloudMsg.header.frame_id = "base_link";
            publisher_.publish(pointCloudMsg);
        }
    }
    
    void Send(sensor_msgs::PointCloud2 &cloud) const;
    BOOL HasSubscriber() const;

private:
    ros::Publisher  publisher_;
    BOOL            hasSubscriber_;
};

/**************************************************************************************
功能描述: 2D坐标数组发布器
修改记录:
**************************************************************************************/
class COOR_2D_ARRAY_MSG_SENDER_C
{
public:
    COOR_2D_ARRAY_MSG_SENDER_C(ros::NodeHandle &nh, string pcTopicName)
    {
        publisher_ = nh.advertise<comm_msg::coor2DArray>(pcTopicName, 1);
    }

    void Send(comm_msg::coor2DArray &coor2DArray);

private:
    ros::Publisher  publisher_;
};

/**************************************************************************************
功能描述: 融合后的静态障碍发布器
修改记录:
**************************************************************************************/
class FUSED_STATIC_OBST_SENDER_C
{
public:
    FUSED_STATIC_OBST_SENDER_C(ros::NodeHandle &nh, const DYNC_CAR_EX_C &objCar)
          : objCar_(objCar),
            objCoor2DArrayMsgSender_(nh, TOPIC_NAME_FUSED_STATIC_OBST),
            objPointCloudMsgSenderByMap_(nh, TOPIC_NAME_FUSED_STATIC_2D_POINTS_BY_MAP),
            objPointCloudMsgSenderByCar_(nh, TOPIC_NAME_FUSED_STATIC_2D_POINTS_BY_CAR)
    {
    }

    ~FUSED_STATIC_OBST_SENDER_C()
    {
    }

    void Send(const list<COORDINATE_2D_STRU> &lstObstByMap, PLAN_MOVE_TREND_ENUM enPlanMoveTrend);

private:
    const DYNC_CAR_EX_C                        &objCar_;

    COOR_2D_ARRAY_MSG_SENDER_C                  objCoor2DArrayMsgSender_;
    POINT_CLOUD_MSG_SENDER_C                    objPointCloudMsgSenderByMap_;   
    POINT_CLOUD_MSG_SENDER_C                    objPointCloudMsgSenderByCar_;
};

/**************************************************************************************
功能描述: 融合后的动态障碍发布器
修改记录:
**************************************************************************************/
class FUSED_DYNAMIC_OBST_SENDER_C
{
public:
    FUSED_DYNAMIC_OBST_SENDER_C(ros::NodeHandle &nh)
    {   
        pubMovTargets_ = nh.advertise<comm_msg::movTargetsArray>(TOPIC_NAME_FUSED_MOV_TARGETS, 1);
        pubMarker_     = nh.advertise<visualization_msgs::Marker>(TOPIC_NAME_MARKER_TARGETS_TRACK_PRED, 20);
    }

    void Send(const comm_msg::movTargetsArray &msgMovTargetsArray);

private:    
    ros::Publisher                      pubMovTargets_;
    ros::Publisher                      pubMarker_;    
};

/**************************************************************************************
功能描述: 激光雷达消息接收器
修改记录:
**************************************************************************************/
class LIDAR_MSG_RECEIVER_C
{
public:
    LIDAR_MSG_RECEIVER_C(ros::NodeHandle &nh, string sensorName)
          : ptrCloud_(new MyCloud()),
            dataSwOldVal_(false),
            dataOutputTimes_(0),
            faultSwOldVal_(false),
            faultOutputTimes_(0),
            objDataInvalidTimer_(0.2, false),
            objSensorFaultTimer_(60.0, false)
    {
        receiver_ = nh.subscribe(sensorName+"/car_filtered_points", 1, &LIDAR_MSG_RECEIVER_C::MsgReceiveCallBack, this);
    }

    ~LIDAR_MSG_RECEIVER_C(){}

    BOOL IsDataInvalid() const;
    BOOL IsSensorFault() const;
    void GetNormalPointsCloud(MyCloud::Ptr &ptrCloud) const;

private:
    ros::Subscriber     receiver_;
    MyCloud::Ptr        ptrCloud_;

    BOOL                dataSwOldVal_;
    UINT32              dataOutputTimes_;
    BOOL                faultSwOldVal_;
    UINT32              faultOutputTimes_;

    TIMER_C             objDataInvalidTimer_;   //超时表示数据无效
    TIMER_C             objSensorFaultTimer_;   //超时表示传感器故障

    void MsgReceiveCallBack(const sensor_msgs::PointCloud2ConstPtr &pstMsg);
    void GetPointsCloud(MyCloud::Ptr &ptrCloud) const;
    void TimerReset();
};

/**************************************************************************************
功能描述: 接收毫米波雷达消息
修改记录:
**************************************************************************************/
class RADAR_MSG_RECEIVER_C
{
public:
    RADAR_MSG_RECEIVER_C(ros::NodeHandle &nh, const char *pcTopicName)
          : objDataInvalidTimer_(0.2, false),
            objSensorFaultTimer_(60.0, false)
    {
        receiver_ = nh.subscribe(pcTopicName, 1, &RADAR_MSG_RECEIVER_C::ReceiveMsgCallBack, this);
    }

    BOOL IsDataInvalid() const;
    BOOL IsSensorFault() const;    
    const RADAR_INFO_STRU *Get(void);

private:
    ros::Subscriber         receiver_;

    TIMER_C                 objDataInvalidTimer_;   //超时表示数据无效
    TIMER_C                 objSensorFaultTimer_;   //超时表示传感器故障

    RADAR_INFO_STRU         stRadarInfo_;

    void ReceiveMsgCallBack(const comm_msg::radarTargetArray::ConstPtr &pstMsg);
    void TimerReset();
};

/**************************************************************************************
功能描述: 深度相机消息接收器
修改记录:
**************************************************************************************/
class DEPTH_CAMERA_MSG_RECEIVER_C
{
public:
    DEPTH_CAMERA_MSG_RECEIVER_C(ros::NodeHandle &nh)
          : nh_(nh),
            ptrCloud_(new pcl::PointCloud<pcl::PointXYZ>),
            dataSwOldVal_(false),
            dataOutputTimes_(0),
            faultSwOldVal_(false),
            faultOutputTimes_(0),
            objDataInvalidTimer_(0.3, false),
            objSensorResetTimer_(30.0, false),
            objSensorFaultTimer_(180.0, false)
    {
        pointsSub_ = nh.subscribe(TOPIC_NAME_VISION_PCLOUD_SRC, 1, &DEPTH_CAMERA_MSG_RECEIVER_C::MsgReceiveCallBack, this);
    }

    ~DEPTH_CAMERA_MSG_RECEIVER_C(){}

    BOOL IsDataInvalid() const;
    BOOL IsSensorReset() const;
    BOOL IsSensorFault() const;
    void TimerReset();
    void GetNormalPointsCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &ptrCloud) const;

private:
    ros::NodeHandle                    &nh_;
    ros::Subscriber                     pointsSub_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr ptrCloud_;

    BOOL                                dataSwOldVal_;
    UINT32                              dataOutputTimes_;
    BOOL                                faultSwOldVal_;
    UINT32                              faultOutputTimes_;

    TIMER_C                             objDataInvalidTimer_;   //超时表示数据无效
    TIMER_C                             objSensorResetTimer_;   //超时表示需要进行传感器复位
    TIMER_C                             objSensorFaultTimer_;   //超时表示传感器故障

    void MsgReceiveCallBack(const sensor_msgs::PointCloud2ConstPtr &pstMsg);
    void GetPointsCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &ptrCloud) const;
};

/**************************************************************************************
功能描述: 选择深度相机接收器
修改记录:
**************************************************************************************/
class SELECT_DEPTH_CAMERA_MSG_RECEIVER_C
{
public:
    SELECT_DEPTH_CAMERA_MSG_RECEIVER_C(ros::NodeHandle &nh)
          : enSelectDepthCamera_(DEPTH_CAMERA_NULL)
    {
        receiver_ = nh.subscribe(TOPIC_NAME_VISION_SELECT_DEPTH_CAMERA, 1, &SELECT_DEPTH_CAMERA_MSG_RECEIVER_C::ReceiveMsgCallBack, this);
    }

    void Get(DEPTH_CAMERA_ENUM &enSelectDepthCamera);

private:
    ros::Subscriber     receiver_;
    DEPTH_CAMERA_ENUM   enSelectDepthCamera_;

    void ReceiveMsgCallBack(const comm_msg::visionDepthCamera::ConstPtr &pstMsg);
};

/**************************************************************************************
功能描述: 相机点云发送器
修改记录:
**************************************************************************************/
class CAMERA_PCD_SENDER_C
{
public:
    CAMERA_PCD_SENDER_C(ros::NodeHandle &nh, const char *pcTopicName)
    {
        publisher_ = nh.advertise<sensor_msgs::PointCloud2>(pcTopicName, 1);
    }

    void Send(pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud);

private:
    ros::Publisher publisher_;
};

/**************************************************************************************
功能描述: 与视觉通信状态发布器
修改记录:
**************************************************************************************/
class VISION_STATE_SENDER_C
{
public:
    VISION_STATE_SENDER_C(ros::NodeHandle &nh)
    {
        publisher_ = nh.advertise<comm_msg::visionState>(TOPIC_NAME_VISION_STATE, 1);
    }

    ~VISION_STATE_SENDER_C(){}

    void Send(UINT08 visionState);
private:
    ros::Publisher publisher_;
};

#endif

